
/*****************************************************************************\
 *                             SphereTopology.cpp
\*****************************************************************************/

/*! @file 
 *
 *  @brief
 *
 *  @author Weiyu Yi, inweiyu@googlemail.com
 */

//---------------------------------------------------------------------------
//  Includes
//---------------------------------------------------------------------------

#include <limits>

#include <defEException.h>

#include "SphereTopology.h"

namespace vhand
{

//***************************************************************************
// Help Functions
//***************************************************************************

/** Test whether 2 spheres are adjacent
 *
 * @param sp1
 * @param sp2
 * 
 * @return 
 */
bool SphereTopology::isAdjacent( const Sphere &sp1, const Sphere &sp2, float threshold )
{
    Point3 sp1centre( sp1.pX, sp1.pY, sp1.pZ );
    Point3 sp2centre( sp2.pX, sp2.pY, sp2.pZ );
    if ( dist( sp1centre, sp2centre ) - ( sp1.mR + sp2.mR ) <= threshold )
    {
        return true;
    }
    else
    {
        return false;
    }
}

/** Find the closest vertex of a mesh to the certain sphere
 * 
 * @param closestdist
 * @param closestindex
 * @param sp
 * @param pnts
 *
 */
void SphereTopology::findClosest( float *closestdist, unsigned int *closestindex, 
                                  const Point3 &pnt3, const std::vector<Sphere> *allsphere )
{
    float dist = std::numeric_limits<float>::max();
    int chosenindex = -1;

    for ( unsigned int i=0; i<allsphere->size(); i++ )
    {
        float newdist = distSqr( Point3( (*allsphere)[i].pX, (*allsphere)[i].pY, (*allsphere)[i].pZ ), pnt3 );
        if ( newdist < dist ) 
        {
            dist = newdist;
            chosenindex = i;
        }
    }

    *closestdist = dist;
    *closestindex = chosenindex;
}

/** Overloaded
 *  Find the closest vertex of a mesh to the certain sphere
 * 
 * @param closestdist
 * @param closestindex
 * @param sp
 * @param pnts
 */
void SphereTopology::findClosest( float *closestdist, unsigned int *closestindex, 
                                  const Sphere &sp, const std::vector<Point3> *pnts )
{
    float dist = std::numeric_limits<float>::max();
    int chosenindex = -1;

    for ( unsigned int i=0; i<pnts->size(); i++ )
    {
        float newdist = distSqr( Point3( sp.pX, sp.pY, sp.pZ ), (*pnts)[i] );
        if ( newdist < dist ) 
        {
            dist = newdist;
            chosenindex = i;
        }
    }

    *closestdist = dist;
    *closestindex = chosenindex;
}


/** 
 * 
 * @param set0
 * @param set1
 * 
 * @return 
 */
std::set<unsigned int> SphereTopology::intersection( const std::set<unsigned int> &set0, const std::set<unsigned int> &set1 )
{
    std::set<unsigned int> result;
    for ( std::set<unsigned int>::const_iterator iter0 = set0.begin(); 
          iter0 != set0.end(); iter0 ++ )
    {
        for ( std::set<unsigned int>::const_iterator iter1 = set1.begin();
              iter1 != set1.end(); iter1 ++ )
        {
            if ( *iter0 == *iter1 )
            {
                result.insert( *iter0 );
            }
        }
    }
    return result;
}

//***************************************************************************
//                            Adjacency
//***************************************************************************

Adjacency::Adjacency( unsigned int p0, unsigned int p1, AdjacencyType type )
:
    _p0( p0 ),
    _p1( p1 ),
    _type( type )
{
}


std::ostream& operator<<( std::ostream &os, const Adjacency &adj )
{
    os << adj.getOut() << " " << adj.getIn() << " ";

    switch ( adj.getType() )
    {
        case MAP_SPHERE_BONES :
        {
            os << "map_sphere_bones" << std::endl;
            break;
        }
        case MAP_SKIN_SPHERE :
        {
            os << "map_skin_sphere" << std::endl;
            break;
        }
        case SKIN_SKIN :
        {
            os << "skin_skin" << std::endl;
            break;
        }
        case SKIN_SPHERE :
        {
            os << "skin_sphere" << std::endl;
            break;
        }
        case SPHERE_SPHERE :
        {
            os << "sphere_sphere" << std::endl;
            break;
        }
        case SPHERE_BONES :
        {
            os << "sphere_bones" << std::endl;
            break;
        }
		default:
        {
            throw XError( "Errors while calling %s : %s :\n",
                          __FILE__, __FUNCTION__ );
			break;
        }
    }

    return os;
}


//***************************************************************************
//                          SphereTopology   
//***************************************************************************


void SphereTopology::initHost( const Geometry *bones_geom,
                               const Geometry *skin_geom,
                               const std::vector<Sphere> *allsphere,
                               float threshold/* = 0.0 */)
{
    unsigned int index;
    float dist;
 
    // for all spheres
    for ( unsigned int i=0; i<allsphere->size(); i++ )
    {
        // sphr <---> sphr
        //
        // The searching progress runs always beginning with the lowest index
        // 0 and incrementing by 1 till end index so if relation 4 <---> 1
        // exists, it should be detected already as 1 <---> 4
        //
        // so the begin index of inner circle could start as the value just
        // very greater the current outer index.
        //
        // No duplicated adjacency exists
        // 

        SphereNeighbours sphrneighbours;
        for ( unsigned int j=i+1; j<allsphere->size(); j++ )
        {
            if ( isAdjacent( (*allsphere)[i], (*allsphere)[j], threshold ) )
            {
                _allAdjacency.push_back( Adjacency( i, j, SPHERE_SPHERE ) );
            }
        }

        // sphr <---> bones
        findClosest( &dist, &index, (*allsphere)[i], bones_geom->getPositions()->getArray() );
        _allAdjacency.push_back( Adjacency( i, index, MAP_SPHERE_BONES ) );
        if ( dist - (*allsphere)[i].mR <= MTD )
        {
            _allAdjacency.push_back( Adjacency( i, index, SPHERE_BONES ) );
        }
    }

    //// skin <----> sphr
    const std::vector<Point3> *skinpos = skin_geom->getPositions()->getArray();
    for ( unsigned int i=0; i<skinpos->size(); i++ )
    {
        findClosest( &dist, &index, (*skinpos)[i], allsphere );
        _allAdjacency.push_back( Adjacency( i, index, MAP_SKIN_SPHERE ) );
        if ( dist - (*allsphere)[index].mR <= MTD )
        {
            _allAdjacency.push_back( Adjacency( i, index, SKIN_SPHERE ) );
        }
    }

    //// skin <----> skin
    const std::vector<unsigned int> *skinprim = skin_geom->getAllPrimitiveSets()->front().getArray();
    unsigned int skinprimsize = 0;
    if ( skin_geom->getAllPrimitiveSets()->front().getType() == GL_TRIANGLES )
    {
        skinprimsize = skinprim->size() / 3;
    }
    else
    {
        throw XError( "Errors while calling %s : %s :\n\tOnly type GL_TRIANGLES supported now.\n",
                      __FILE__, __FUNCTION__ );
    }

    std::vector< std::set<unsigned int> > skinAdjacencyList( skin_geom->getPositions()->getArray()->size() );
    unsigned int adjc_skinskindirect_startindex = _allAdjacency.size();
    for ( unsigned int i=0; i<skinprimsize; i++ )
    {
        unsigned int ti0 = (*skinprim)[ 0+3* i ];
        unsigned int ti1 = (*skinprim)[ 1+3* i ];
        unsigned int ti2 = (*skinprim)[ 2+3* i ];

        skinAdjacencyList[ ti0 ].insert( ti1 );
        skinAdjacencyList[ ti0 ].insert( ti2 );
        skinAdjacencyList[ ti1 ].insert( ti2 );
        skinAdjacencyList[ ti1 ].insert( ti0 );
        skinAdjacencyList[ ti2 ].insert( ti0 );
        skinAdjacencyList[ ti2 ].insert( ti1 );

        if ( ti0 < ti1 ) _allAdjacency.push_back( Adjacency( ti0, ti1, SKIN_SKIN ) );
        if ( ti1 < ti2 ) _allAdjacency.push_back( Adjacency( ti1, ti2, SKIN_SKIN ) );
        if ( ti2 < ti0 ) _allAdjacency.push_back( Adjacency( ti2, ti0, SKIN_SKIN ) );
    }
    unsigned int adjc_skinskindirect_endindex = _allAdjacency.size();
    for ( unsigned int i= adjc_skinskindirect_startindex; i < adjc_skinskindirect_endindex; i ++ )
    {
        unsigned int t0 = _allAdjacency[i].getOut();
        unsigned int t1 = _allAdjacency[i].getIn();

        std::set<unsigned int> interset = intersection( skinAdjacencyList[t0], skinAdjacencyList[t1] );
        if ( interset.size() == 2 )
        {
            std::set<unsigned int>::const_iterator iter = interset.begin();
            unsigned int newout = *iter;
            iter ++;
            unsigned int newin = *iter;
            _allAdjacency.push_back( Adjacency( newout, newin, SKIN_SKIN ) );
        }
        else
        {
            printf( " error! size of intersection set should have only 2 elements\n" );
            printf( " size of intersection is %d. [ ", interset.size() );

            for (  std::set<unsigned int>::const_iterator iter = interset.begin();
                   iter != interset.end(); iter ++ )
            {
                printf( "%d, ", *iter );
            }
            printf( "]\n" );
        }
    }
}

} // namespace vhand

